% directly from human data
% calculate 3D outputs 

addpath('./Func/')
addpath('./HumanData')
% name list 9 person
name  = {'Charles','Fred','Martin','Ram','Selina',... % 1st experiment
    'lily','po','ryan','vic'};  % 2nd experiment

% CONSTANT
% the number of subjects from the 1st experiment
exp1st = 5;
% the index of subjects use the right leg as swing leg during the step
rIndex = [6,7,9];
% experimental sampling frequency
fs = 480;
% % model = 'NAO';
model = 'HM';
% model  = 'AMBER';
time_rec = zeros(9,1);
LT = 0; % the length of the torso
whip = 0; % the width of hip
%
for index = 1:length(name) % 5

    % get human data, with breaking pts for each step
    humanData = getData(name{1,index});
   
    %     sum_weight  = sum_weight+humanData.weight; % for average weight
    % breaking pts for each step
    StartPt = humanData.sns_brk(2);
    EndPt = humanData.sns_brk(3);
    
    % position
    if index <= exp1st
        [xpos,ypos,zpos] = getPosCartesian(humanData); % subjects from experiment 1st
    else
        [xpos,ypos,zpos] = getPosCartesianNew3D(humanData); % subjects from experiment 2st
    end
    
        % calculate the length of torso
    Torso = calDist(xpos.hip_avg,ypos.hip_avg,zpos.hip_avg,...
        xpos.belly,ypos.belly,zpos.belly);
    LT = LT+Torso;
    HipWidth = calDist(xpos.lhip,ypos.lhip,zpos.lhip,...
        xpos.rhip,ypos.rhip,zpos.rhip);
    whip = whip+HipWidth;
    
    % 
    hip_disp = (HipWidth - humanData.hipjoint)/2;
    % z direction
            zpos.lhip = zpos.lhip - hip_disp;
            zpos.rhip = zpos.rhip + hip_disp;
    %%%%%%%%%%%  time  %%%%%%%%%%%%%%
    % scale time
    % from [c:d]
    ns_time_pre = (0:length(StartPt:EndPt)-1)'/fs;
    ns_ret = ns_time_pre;
    xpara_time = 1/ns_ret(end); % simply sacled time to 1
    ns_time = ns_ret*xpara_time;
    %%%% record time %%%%%%%%
    time_rec(index) = ns_ret(end);
    
    
    
    %%%%%%%%%%%% calculate human joint angles %%%%%%%%%%%%%%%%%
    if ismember(index,rIndex)

        angle = Knee3D_angle(xpos,ypos,zpos,humanData.sns_brk,2);
        COM = cal_CoM3D(xpos,ypos,zpos,humanData,0);
        slope = Knee3D_slope(xpos,ypos,zpos,humanData.sns_brk,2);
    else
        angle = Knee3D_angle(xpos,ypos,zpos,humanData.sns_brk,1);
        COM = cal_CoM3D(xpos,ypos,zpos,humanData,1);
        slope = Knee3D_slope(xpos,ypos,zpos,humanData.sns_brk,1);
    end
    plot(COM.angle); hold on;
    %%%%%%%%%%%% calculate human slope %%%%%%%%%%
%     if ismember(index,rIndex)
%         slope = Knee3D_slope(xpos,ypos,zpos,humanData.sns_brk,2);
%     else
%         slope = Knee3D_slope(xpos,ypos,zpos,humanData.sns_brk,1);
%     end
    
    % 1. stance ankle roll angle
    [ave_dataSA,ave_timeSA] = BeamData(ns_time,angle.sa);
    ave_dataSA_s(index,:) = ave_dataSA;
    
    % 2. stance hip roll angle
    [ave_dataShip,ave_timeShip] = BeamData(ns_time,angle.ship);
    ave_dataShip_s(index,:) = ave_dataShip;

    % 3. non-stance hip roll angle
    [ave_dataNShip,ave_timeNShip] = BeamData(ns_time,angle.nship);
    ave_dataNShip_s(index,:) = ave_dataNShip;
    
    
    % 4. torso roll angle
    [ave_dataTorso,ave_timeTorso] = BeamData(ns_time,angle.torso);
    ave_dataTorso_s(index,:) = ave_dataTorso;
    
    % 5. hip y position
    [ave_dataHipy,ave_timeHipy] = BeamData(ns_time,slope.hipy);
    ave_dataHipy_s(index,:) = ave_dataHipy;
    
    % 6. stance hip y position
    [ave_dataSHipy,ave_timeSHipy] = BeamData(ns_time,slope.shipy);
    ave_dataSHipy_s(index,:) = ave_dataSHipy;
    
    % 7. non-stance hip y position
    [ave_dataNSHipy,ave_timeSHipy] = BeamData(ns_time,slope.nshipy);
    ave_dataNSHipy_s(index,:) = ave_dataNSHipy;
    
    % 8. non-stance ankle y position
    [ave_dataNSAy,ave_timeNSAy] = BeamData(ns_time,slope.nsay);
    ave_dataNSAy_s(index,:) = ave_dataNSAy;
end
ave_time = ave_timeSA;
LT = LT/9;
whip = whip/9;
%% cal;culate the mean value and save
%%%%%%%%%%%% angles %%%%%%%%%%%%%%%%%%%%%%%%%

% 1. stance ankle angle
[x_meanSA,upperBSA,lowerBSA]=meanValue(ave_dataSA_s);
% 2. stance hip angle
[x_meanShip,upperBShip,lowerBShip]=meanValue(ave_dataShip_s);
% 3. non-stance hip angle
[x_meanNShip,upperBNShip,lowerBNShip]=meanValue(ave_dataNShip_s);
% 4. torso angle
[x_meanTorso,upperBTorso,lowerBTorso]=meanValue(ave_dataTorso_s);

% 5. hip position on z direction
[x_meanHipy,upperBHipy,lowerBHipy]=meanValue(ave_dataHipy_s);

% 6. ship position on y direction
[x_meanSHipy,upperBSHipy,lowerBSHipy]=meanValue(ave_dataSHipy_s);

% 7. nship position on y direction
[x_meanNSHipy,upperBNSHipy,lowerBNSHipy]=meanValue(ave_dataNSHipy_s);

% 8. nsankle position on y direction
[x_meanNSAy,upperBNSAy,lowerBNSAy]=meanValue(ave_dataNSAy_s);


% save the data
 ns_time_mean = mean(time_rec)*ave_time'; % averaged time
%% 
 data_name = 'mean_data';
 folder_name = ['data_' model '_3D'];
%  save_outputs(Xmean,ns_time_mean,data_name,folder_name);
%%%%%%%%%%%% angles %%%%%%%%%%%%%%%%
ns_time = ns_time_mean;
 % 1. stance ankle angle
s_ankle_3D =x_meanSA';
save(['./', folder_name, '/',data_name,'_sankle3D.mat'],'s_ankle_3D','ns_time')

 % 2. stance hip angle
s_hip_3D = x_meanShip';
save(['./', folder_name, '/', data_name,'_ship3D.mat'],'s_hip_3D','ns_time');

% 3. non-stance hip angle
ns_hip_3D = x_meanNShip';
save(['./', folder_name, '/', data_name,'_nship3D.mat'],'ns_hip_3D','ns_time');

% 4. torso angle
torso_3D = x_meanTorso';
save(['./', folder_name, '/', data_name,'_torso3D.mat'],'torso_3D','ns_time');

% 5. hip position on z direction
hip_y = x_meanHipy';
save(['./', folder_name, '/', data_name,'_HipY.mat'],'hip_y','ns_time');

%% 6. ship position on y direction
ship_y = x_meanSHipy';
save(['./', folder_name, '/', data_name,'_SHipY.mat'],'ship_y','ns_time');

% 7. nship position on y direction
nship_y = x_meanNSHipy';
save(['./', folder_name, '/', data_name,'_NSHipY.mat'],'nship_y','ns_time');

% 8. nsankle position on y direction
nsa_y = x_meanNSAy';
save(['./', folder_name, '/', data_name,'_NSAY.mat'],'nsa_y','ns_time');
